#include "control.h"
#include "Rc.h"
#include "imu.h"
#include "tim_pwm_out.h"
PID PID_ROL,PID_PIT,PID_YAW,PID_ALT,PID_POS,PID_PID_1,PID_PID_2;

int16_t getlast_roll=0,geilast_pitch=0;
float rol_i=0,pit_i=0,yaw_p=0;

vs16 Moto_PWM_1=0,Moto_PWM_2=0,Moto_PWM_3=0,Moto_PWM_4=0,Moto_PWM_5=0,Moto_PWM_6=0,Moto_PWM_7=0,Moto_PWM_8=0;
void CONTROL(float rol, float pit, float yaw)
{

	rol=rol-(Rc_Data.ROLL-1500)/20;

	rol_i+=rol;
	if(rol_i>2000)
	rol_i=2000;
	if(rol_i<-2000)
	rol_i=-2000;

	PID_ROL.pout = PID_ROL.P * rol;
	PID_ROL.dout = PID_ROL.D * MPU6050_GYRO_LAST.X;
	PID_ROL.iout = PID_ROL.I * rol_i;

	pit=pit+(Rc_Data.PITCH-1500)/20;
	
	pit_i+=pit;
	if(pit_i>2000)
	pit_i=2000;
	if(pit_i<-2000)
	pit_i=-2000;

	PID_PIT.pout = PID_PIT.P * pit;
	PID_PIT.dout = PID_PIT.D * MPU6050_GYRO_LAST.Y;
	PID_PIT.iout = PID_PIT.I * pit_i;
	if(Rc_Data.YAW<1480||Rc_Data.YAW>1520)
	{MPU6050_GYRO_LAST.Z=MPU6050_GYRO_LAST.Z+(Rc_Data.YAW-1500)*10;}
	yaw_p+=MPU6050_GYRO_LAST.Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30


	PID_YAW.pout=PID_YAW.P*yaw_p;
	PID_YAW.dout = PID_YAW.D * MPU6050_GYRO_LAST.Z;				   
	
if(Rc_Data.THROTTLE<1200)
		{		
		pit_i=0;
		rol_i=0;
		yaw_p=0;
		}

	PID_ROL.OUT = (-PID_ROL.pout) - PID_ROL.iout +PID_ROL.dout;
	PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
	PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
 
	if(Rc_Data.THROTTLE>1200&&ARMED)
	{
		Moto_PWM_1 = Rc_Data.THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
		Moto_PWM_2 = Rc_Data.THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
		Moto_PWM_3 = Rc_Data.THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;
		Moto_PWM_4 = Rc_Data.THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
	}
	else
	{
		Moto_PWM_1 = 0;
		Moto_PWM_2 = 0;
		Moto_PWM_3 = 0;
		Moto_PWM_4 = 0;

	}
	Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
				
}
